"""
src/linact.py
    Create a Linear Actuator class derived from the ode SliderJoint.
    This class provides all the hooks necessary to have a fully featured
    joint without a lot of work by the user.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
"""

from pandac.PandaModules import OdeSliderJoint
Infinity = 1e10000
NaN = Infinity / Infinity

class LinearActuator(OdeSliderJoint):
    """ Can behave like a spring coil, shock absorber, linear position actuator,
    a combination of all 3. It provides no rotational constraints at all."""
    # Provide new so we can have subclasses that add parameters
    def __new__(cls, *args, **kwargs):
        # Initialize the SliderJoint class
        return OdeSliderJoint.__new__(cls, args[0]) # First arg is World

    def __init__(self, World):
        # Initialize the LinearActuator class
        OdeSliderJoint.__init__(self, World)

        # Initialize the properties to benign values
        self._minSpringForce = 0.0
        self._maxSpringForce = 0.0

        self._lockedForceLimit = 1.0e99
        self._staticFrictionForce = 0.0
        self._kineticFrictionForce = 0.0
        self._frictionTol = 0.0001

        self._externalForce = 0.0
        self._isLocked = False

        self._connectionIDs = [None, None]

        # Set the linear limits
        self.setParamLoStop(-Infinity)
        self.setParamHiStop(Infinity)

        # Make the stops non bouncy
        self.setParamBounce(0.0)

        # Set the target motor rate to 0 m/sec
        self.setParamVel(0.0)

        # Set the motor to the static friction force
        self.setParamFMax(self._staticFrictionForce)

    def GetProperties(self):
        properties = (self._minSpringForce, self._maxSpringForce, self._lockedForceLimit,
                        self._staticFrictionForce, self._kineticFrictionForce, self._frictionTol,
                        self.getParamLoStop(), self.getParamHiStop())

        return properties

    def ChangeProperties(self, MinSpringForce=None, MaxSpringForce=None, LockedForceLimit=None,
                            StaticFrictionForce=None, KineticFrictionForce=None,
                            FrictionTol=None, MinStop=None, MaxStop=None):
        # Make changes to specified properties
        if MinSpringForce is not None:
            self._minSpringForce = MinSpringForce
        if MaxSpringForce is not None:
            self._maxSpringForce = MaxSpringForce
        if LockedForceLimit is not None:
            self._lockedForceLimit = LockedForceLimit
        if StaticFrictionForce is not None:
            self._staticFrictionForce = StaticFrictionForce
        if KineticFrictionForce is not None:
            self._kineticFrictionTorque = KineticFrictionForce
        if FrictionTol is not None:
            self._frictionTol = FrictionTol
        if MinStop is not None:
            self.setParamLoStop(MinStop)
        if MaxStop is not None:
            self.setParamHiStop(MaxStop)

    def GetExternalForce(self):
        """Get the external force currently being applied to the slider joint."""
        return self._externalForce

    def SetExternalForce(self, Force):
        """Set the external force to apply to the slider joint.  This is applied
        along with spring and frictional forces every time UpdateSettings() is called."""
        self._externalForce = Force

    def UpdateSettings(self): # Called before the next time step
        """This function must be called before each time step is executed.  It computes
        and updates the forces applied to the joint.  It also handles joint locking."""
        if self._isLocked:
            self.setParamFMax(self._lockedForceLimit)
        else:
            # Set the motor to the static friction torque
            self.setParamFMax(self._staticFrictionForce)

        # Compute spring force
        pos = self.getPosition()
        minPos = self.getParamLoStop()
        maxPos = self.getParamHiStop()
        if pos < minPos:
            springForce = self._minSpringForce
        elif pos > maxPos:
            springForce = self._maxSpringForce
        else:
            springForce = self._minSpringForce + (self._maxSpringForce - self._minSpringForce) * (pos - minPos)

        # Compute kinetic friction force
        linearVelocity = self.getPositionRate()

        if linearVelocity < -self._frictionTol:
            kineticFrictionForce = self._kineticFrictionForce
        elif linearVelocity > self._frictionTol:
            kineticFrictionForce = -self._kineticFrictionForce
        else:
            kineticFrictionForce = 0.0

        self.addForce(springForce + kineticFrictionForce + self._externalForce)

        #print self.GetProperties()

    def Lock(self):
        """Lock the joint so it will not slide."""
        self._isLocked = True

    def Unlock(self):
        """Unlock the joint to allow it to slide."""
        self._isLocked = False

